﻿using FRRJIf;
using RJi.FanucInterface3.Common.Internal;
using RJi.FanucInterface3.Models;

namespace RJi.FanucInterface3.Helper;

internal static class PosParserHelper

{
    public static object? DataCurPosParser(VariableMetadata metadata)
    {
        Array xyzwpr = new float[9];
        Array config = new short[7];
        Array joint = new float[9];
        short uf = 0, ut = 0, validC = 0, validJ = 0;

        var curPos = (DataCurPos)metadata.VariableRef;
        if (!curPos.GetValue(ref xyzwpr, ref config, ref joint, ref uf, ref ut, ref validC, ref validJ))
            return null;

        // 检查坐标有效性：如果笛卡尔或关节坐标无效（intValidC 或 intValidJ 为 0），返回 null
        if (validC == 0 || validJ == 0)
            return null;

        // 转换为具体类型
        var cartesian = (float[])xyzwpr;
        var jointArr = (float[])joint;
        var configArr = (short[])config;

        return new RbtPosition(
            uf,
            ut,
            new RbtJointPosition(jointArr[0], jointArr[1], jointArr[2], jointArr[3], jointArr[4], jointArr[5], jointArr[6], jointArr[7], jointArr[8]),
            new RbtCartesianPosition(
                cartesian[0], cartesian[1], cartesian[2], cartesian[3], cartesian[4], cartesian[5], cartesian[6], cartesian[7], cartesian[8]),
            Array.AsReadOnly(ParseConfig(configArr)));
    }

    public static object? DataPosRegParser(VariableMetadata metadata)
    {
        var posReg = (DataPosReg)metadata.VariableRef;
        var rbtPos = new RbtRegVar[metadata.Count];

        for (var i = 0; i < metadata.Count; i++)
        {
            Array xyzwpr = new float[9];
            Array config = new short[7];
            Array joint = new float[9];
            short uf = 0, ut = 0, validC = 0, validJ = 0;

            var index = i + metadata.StartIndex;

            if (!posReg.GetValue(index, ref xyzwpr, ref config, ref joint, ref uf, ref ut, ref validC, ref validJ)) return null;
            if (validC == 0 || validJ == 0)
            {
                rbtPos[i] = null!;
                continue;
            }

            var cartesian = (float[])xyzwpr;
            var jointArr = (float[])joint;
            var configArr = (short[])config;

            var pos = new RbtPosition(
                uf,
                ut,
                new RbtJointPosition(jointArr[0], jointArr[1], jointArr[2], jointArr[3], jointArr[4], jointArr[5], jointArr[6], jointArr[7], jointArr[8]),
                new RbtCartesianPosition(
                    cartesian[0], cartesian[1], cartesian[2], cartesian[3], cartesian[4], cartesian[5], cartesian[6], cartesian[7], cartesian[8]),
                Array.AsReadOnly(ParseConfig(configArr)));

            rbtPos[i] = new RbtRegVar(index, pos);
        }

        return rbtPos;
    }

    public static object? DataSysVarPosArrayParser(VariableMetadata metadata)
    {
        // 如果是数组返回以下内容
        var sysVarPos = (DataSysVarPos[])metadata.VariableRef;
        var rbtPos = new RbtPosition[metadata.Count];

        for (var i = 0; i < metadata.Count; i++)
        {
            Array xyzwpr = new float[9];
            Array config = new short[7];
            Array joint = new float[9];
            short uf = 0, ut = 0, validC = 0, validJ = 0;

            if (!sysVarPos[i].GetValue(ref xyzwpr, ref config, ref joint, ref uf, ref ut, ref validC, ref validJ)) return null;
            if (validC == 0 && validJ == 0)
            {
                rbtPos[i] = null!;
                continue;
            }

            var cartesian = (float[])xyzwpr;
            var jointArr = (float[])joint;
            var configArr = (short[])config;

            rbtPos[i] = new RbtPosition(
                uf,
                ut,
                new RbtJointPosition(jointArr[0], jointArr[1], jointArr[2], jointArr[3], jointArr[4], jointArr[5], jointArr[6], jointArr[7], jointArr[8]),
                new RbtCartesianPosition(
                    cartesian[0], cartesian[1], cartesian[2], cartesian[3], cartesian[4], cartesian[5], cartesian[6], cartesian[7], cartesian[8]),
                Array.AsReadOnly(ParseConfig(configArr)));
        }

        return Array.AsReadOnly(rbtPos);
    }

    public static object? DataSysVarPosParser(VariableMetadata metadata)
    {
        Array xyzwpr = new float[9];
        Array config = new short[7];
        Array joint = new float[9];
        short uf = 0, ut = 0, validC = 0, validJ = 0;

        var sysVarPos = (DataSysVarPos)metadata.VariableRef;
        if (!sysVarPos.GetValue(ref xyzwpr, ref config, ref joint, ref uf, ref ut, ref validC, ref validJ))
            return null;

        // 检查坐标有效性：如果笛卡尔或关节坐标无效（intValidC 或 intValidJ 为 0），返回 null
        if (validC == 0 && validJ == 0)
            return null;

        // 转换为具体类型
        var cartesian = (float[])xyzwpr;
        var jointArr = (float[])joint;
        var configArr = (short[])config;

        return new RbtPosition(
            uf,
            ut,
            new RbtJointPosition(jointArr[0], jointArr[1], jointArr[2], jointArr[3], jointArr[4], jointArr[5], jointArr[6], jointArr[7], jointArr[8]),
            new RbtCartesianPosition(
                cartesian[0], cartesian[1], cartesian[2], cartesian[3], cartesian[4], cartesian[5], cartesian[6], cartesian[7], cartesian[8]),
            Array.AsReadOnly(ParseConfig(configArr)));
    }

    private static string[] ParseConfig(short[] config)
    {
        return
        [
            config[0] != 0 ? "F " : "N ",
            config[1] != 0 ? "L " : "R ",
            config[2] != 0 ? "U " : "D ",
            config[3] != 0 ? "T " : "B ",
            config[4].ToString(),
            config[5].ToString(),
            config[6].ToString()
        ];
    }
}